#pragma once

#include <numeric>
#include "cartographer/rcm/common.h"

namespace cartographer
{
    class Pose
    {
    public:
        using Ptr = std::shared_ptr<Pose>;

        Pose()
        {
        }

        virtual ~Pose()
        {
        }

        virtual Eigen::Matrix3d Rotation() = 0;

        virtual Eigen::Vector3d Translation() = 0;

        virtual Eigen::Vector3d TranslationDelta(const Pose::Ptr &pose) = 0;

        virtual double Distance(const Pose::Ptr &pose) = 0;

        virtual Pose::Ptr Transform(const Pose::Ptr &pose) = 0;

        virtual double GaussProbability(const Pose::Ptr &pose, Eigen::Vector3d sigma)
        {
            return 0.0;
        }
    };

    class Pose2d : public Pose
    {
    public:
        using Ptr = std::shared_ptr<Pose2d>;

        Pose2d()
        {
            data_.setZero();
        }

        Pose2d(Eigen::Vector3d data)
            : data_(data)
        {
        }

        Pose2d(const Pose2d &pose)
            : data_(pose.data_)
        {
        }

        void Update(Eigen::Vector3d velocity, double delta_t)
        {
            double yawCurr = data_(2) + velocity(2) * delta_t;
            data_(0) = data_(0) + (velocity(0) / velocity(2)) * (std::sin(yawCurr) - std::sin(data_(2)));
            data_(1) = data_(1) + (velocity(1) / velocity(2)) * (std::cos(data_(2)) - std::cos(yawCurr));
            data_(2) = yawCurr;
        }

        Eigen::Vector3d data()
        {
            return data_;
        }

        void Set(Eigen::Vector3d data)
        {
            data_ = data;
        }

        virtual double Distance(const Pose::Ptr &pose)
        {
            auto pose2d = std::dynamic_pointer_cast<Pose2d>(pose);
            Eigen::Vector3d delta = data_ - pose2d->data_;
            return delta.head(2).norm();
        }

        Eigen::Vector3d Translation()
        {
            return Eigen::Vector3d(data_(0), data_(1), 0);
        }

        Eigen::Vector3d TranslationDelta(const Pose::Ptr &pose)
        {
            auto pose2d = std::dynamic_pointer_cast<Pose2d>(pose);
            Eigen::Vector3d delta = data_ - pose2d->data_;
            delta(2) = 0.0;
            return delta;
        }

        Eigen::Matrix3d Rotation()
        {
            Eigen::MatrixXd rotate = Eigen::MatrixXd::Zero(3, 3);
            rotate << std::cos(data_(2)), -std::sin(data_(2)), 0, std::sin(data_(2)), std::cos(data_(2)), 0, 0, 0, 0;
            return rotate;
        }

        Pose::Ptr Transform(const Pose::Ptr &pose)
        {
            auto vector = pose->Rotation() * Translation() + pose->Translation();
            return std::make_shared<Pose2d>(vector);
        }

        double GaussProbability(const Pose::Ptr &pose, Eigen::Vector3d sigma)
        {
            Eigen::Vector3d delta = TranslationDelta(pose);
            double num = std::exp(-0.5 * (std::pow(delta(0), 2) / std::pow(sigma(0), 2) + std::pow(delta(1), 2) / std::pow(sigma(1), 2)));
            double denom = 2 * M_PI * sigma(0) * sigma(1);
            return num / denom;
        }

        Eigen::Vector3d data_;
    };
} // namespace cartographer
